#include <clustering_speed_test/cluster_base.h>
#include <clustering_speed_test/occupancy_cluster.h>
#include <clustering_speed_test/euclidean_cluter.h>
#include <clustering_speed_test/dbscan_cluster.h>

#include <ros/ros.h>

#include <memory>
#include <iostream>
#include <chrono>
#include <glog/logging.h>
#include <yaml-cpp/yaml.h>

int main(int argc,char** argv) {
  google::InitGoogleLogging(argv[0]);
  FLAGS_alsologtostderr = true;
  ros::init(argc,argv,"main_node");

  ros::NodeHandle nh;
  
  YAML::Node node = YAML::LoadFile(std::string(PROJECT_PATH) + "/config/config.yaml");
  std::cout << "PROJECT_PATH : " << PROJECT_PATH << std::endl;
  std::shared_ptr<Cluster_base> cluster;
  if(node["clustering_method"].as<std::string>() == "occupancy_cluster") {
     cluster = std::make_shared<Occupancy_cluster>(nh,node);
  }
  else if(node["clustering_method"].as<std::string>() == "euclidean_cluster") {
    cluster = std::make_shared<Euclidean_cluster>(nh,node);
  }
  else if(node["clustering_method"].as<std::string>() == "dbscan_cluster") {
    cluster = std::make_shared<DBSCAN_cluster>(nh,node);
  }
  else {
    LOG(ERROR) << "Invalid Cluster : " << node["clustering_method"];
  }
  // ros::Subscriber sub_cloud = nh.subscribe<sensor_msgs::PointCloud2>("/rslidar_poinst",&Occupancy_cluster::cloud_callback,&(*cluster))
  ros::spin();
  
}
